#!/usr/bin/env python


import roslib; roslib.load_manifest('FollowBlobs')
import rospy
import message_filters, sensor_msgs, actionlib, tf
import image_geometry
from geometry_msgs.msg import PoseStamped, Point, Pose, PointStamped
from sensor_msgs.msg import PointCloud, Image, JointState, CameraInfo
from stereo_msgs.msg import DisparityImage
from cmvision.msg import Blobs

class FollowBlobs:

	def __init__(self):
		self.right_camera_info = message_filters.Subscriber('/wide_stereo/right/camera_info', CameraInfo)
		self.left_camera_info = message_filters.Subscriber('/wide_stereo/left/camera_info', CameraInfo)
		self.disparity = message_filters.Subscriber('/wide_stereo/disparity', DisparityImage)
		self.strCamConv = image_geometry.StereoCameraModel()
		subscribers = [self.left_camera_info, self.right_camera_info]

		blobs = rospy.Subscriber("blobsLeft",Blobs, self.blobRecieved)
		#ros::Subscriber blobDetectorL = nh.subscribe<cmvision::Blobs> ("blobsLeft",1,lBlobCallback);

		self.time_synch = message_filters.TimeSynchronizer(subscribers, 5)
		self.time_synch.registerCallback(self.cameraInfoCallback)

	def blobRecieved(self, blobs):
		if blobs.blob_count == 0:
			return

		rospy.loginfo("Found Left Blobs:  %i" % blobs.blob_count)
		"""
		unsigned int bestIndex = 0;
		for ( unsigned int i = 0; i < msg->blob_count; i++ ) {
			if (msg->blobs[bestIndex].area < msg->blobs[i].area) {
				bestIndex = i;
			}
		}
			"""

		#lBlob = msg->blobs[bestIndex];
 		#lBlobRecieved = true;

	def cameraInfoCallback(self, leftInfo, rightInfo):
		rospy.loginfo("cameraCallback")
		self.strCamConv.fromCameraInfo(leftInfo, rightInfo)

if __name__ == '__main__':
	rospy.logwarn('Starting Blob Follower')
	rospy.init_node('followBlobs')
	FollowBlobs()
	rospy.spin()
		
